#include "gyro.h"

//#include "main.h"
#include "board.h"
//unsigned short   gyro[3];          // raw data from gyro 
unsigned short   gyro_zero[3];     // zero rate code
float            gyro_scale[3];    // gyro convertion scale to rad/s

//void pqr_update(void)
//{
  //pqr[0] = IMU_ADC_ROLL_DOT_SIGN  ((float)(gyro[0] - gyro_zero[0])) * gyro_scale[0];
  //pqr[1] = IMU_ADC_PITCH_DOT_SIGN ((float)(gyro[1] - gyro_zero[1])) * gyro_scale[1];
  //pqr[2] = IMU_ADC_YAW_DOT_SIGN   ((float)(gyro[2] - gyro_zero[2])) * gyro_scale[2];
//}

void gyro_init(void)
{
  // gyro_zero initialization
  gyro_zero[0] = IMU_ADC_ROLL_DOT_ZERO;
  gyro_zero[1] = IMU_ADC_PITCH_DOT_ZERO;
  gyro_zero[2] = IMU_ADC_YAW_DOT_ZERO;
	
  // gyro_scale initialization
  gyro_scale[0] = IMU_ADC_ROLL_DOT_SCALE;
  gyro_scale[1] = IMU_ADC_PITCH_DOT_SCALE;
  gyro_scale[2] = IMU_ADC_YAW_DOT_SCALE;
}

void gyro_update_x(unsigned short gyro_raw_x)
{
  pqr[0] = IMU_ADC_ROLL_DOT_SIGN  ((float)(gyro_raw_x - gyro_zero[0])) * gyro_scale[0];
}

void gyro_update_y(unsigned short gyro_raw_y)
{
  pqr[1] = IMU_ADC_PITCH_DOT_SIGN  ((float)(gyro_raw_y - gyro_zero[1])) * gyro_scale[1];
}

void gyro_update_z(unsigned short gyro_raw_z)
{
  pqr[2] = IMU_ADC_YAW_DOT_SIGN  ((float)(gyro_raw_z - gyro_zero[2])) * gyro_scale[2];
}
